
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <sstream>
#include <math.h>
#include <vector>
#include <algorithm>
#include <GL\glew.h>

#include "Matrix4.h"
#include "MatrixHelpers.h"

namespace Krengine
{
	/**
	 * create zero-matrix
	 */
	Matrix4::Matrix4( )
		: data(16, 0)
	{
	}


	Matrix4::Matrix4( const std::vector<float>& input )
		: data(16, 0)
	{
		if ( input.size() != 16 )
		{
			std::cout << "Warning: Matrix4 input size != 16 => defaulting to zero-matrix" << std::endl;
			return;
		}

		data = input;
	}


	/**
	 * transposes the matrix
	 */
	void Matrix4::transpose()
	{
		for ( int y = 0; y < 4; y++ )
			for ( int x = y; x < 4; x++ )
				std::swap( data[y * 4 + x], data[x * 4 + y] );
	}


	/**
	 * calculates the inverse of the matrix and saves the resulting matrix
	 * returns false if the matrix can't be inverted
	 */
	bool Matrix4::inverse()
	{
		Matrix4 temp;
		/* create matrix of minors */
		createMinorMatrix(temp.data);

		/* calculate determinant */
		float det = ( data[0] * temp.data[0] )
					 - ( data[1] * temp.data[1] )
					 + ( data[2] * temp.data[2] )
					 - ( data[3] * temp.data[3] );

		/* can't inverse matrix if determinant is 0 */
		if ( det == 0.0f )
			return false;

		/* turn minor into cofactors */
		std::vector<float>::iterator it = temp.data.begin();
		for ( int y = 0; y < 4; y++ )
		{
			for ( int x = 0; x < 4; x++ )
			{
				*it *= std::pow( -1.0f, y + x );
				it++;
			}
		}


		/* find the adjoint of the matrix */
		temp.transpose();

		/* divide the adjoint matrix by the determinant to find the inverse matrix */
		for ( int i = 0; i < 16; i++ )
			temp.data[i] /= det;

		/* save new data */
		data = temp.data;

		return true;
	}


	/**
	 * outputs matrix data
	 */
	const std::string Matrix4::toString() const
	{
		/* stringstream to build the string */
		std::stringstream ss( std::stringstream::in | std::stringstream::out );

		ss << "{" << std::endl;

		for ( int i = 0; i < 16; i++ )
		{
			ss << data[i];

			if ( i % 4 != 3 )
				ss << ",";
			else ss << std::endl;
		}

		ss << "}";

		return std::string(ss.str());
	}


	/****************************** operator overloads ******************************/
	/**
	 * adds one matrix to the other
	 */
	const Matrix4 Matrix4::operator+ ( const Matrix4& m2 )
	{
		Matrix4 temp;

		for ( int i = 0; i < 16; i++ )
			temp.data[i] = data[i] + m2.data[i];

		return Matrix4(temp);
	}


	/**
	 * subtracts one matrix from the other
	 */
	const Matrix4 Matrix4::operator- ( const Matrix4& m2 )
	{
		Matrix4 temp;

		for ( int i = 0; i < 16; i++ )
			temp.data[i] = data[i] - m2.data[i];

		return Matrix4(temp);
	}


	/**
	 * multiplies one matrix by the other
	 */
	const Matrix4 Matrix4::operator* ( const Matrix4& m2 )
	{
		Matrix4 temp;
		for ( int i = 0; i < 4; i++ )
		{
			for( int j = 0; j < 4; j++ )
			{
				temp.data[j + (i * 4)] = (data[i * 4] * m2.data[j]) +
										(data[(i * 4) + 1] * m2.data[j + 4]) +
										(data[(i * 4) + 2] * m2.data[j + 8]) +
										(data[(i * 4) + 3] * m2.data[j + 12]);
			}
		}

		return Matrix4(temp);
	}


	/**
	 * divides one matrix by the other
	 * returns *this if the division can't be done
	 */
	const Matrix4 Matrix4::operator/ ( const Matrix4& m2 )
	{
		Matrix4 temp = m2;

		if ( temp.inverse() )
			return (*this * temp);

		return *this;
	}


	/*const Matrix4 Matrix4::operator+= ( const Matrix4& m2 )
	{
		return (*this + m2);
	}


	const Matrix4 Matrix4::operator-= ( const Matrix4& m2 )
	{
		return (*this - m2);
	}


	const Matrix4 Matrix4::operator*= ( const Matrix4& m2 )
	{
		return (*this * m2);
	}
	
	
	const Matrix4 Matrix4::operator/= ( const Matrix4& m2 )
	{
		return (*this / m2);
	}*/


	/****************************** private functions ******************************/
	/**
	 * creates a matrix of minors from the matrixdata
	 */
	void Matrix4::createMinorMatrix( std::vector<float>& minorContainer ) const
	//void Matrix4::createMinorMatrix( GLfloat *minorContainer ) const
	{
		int row, col, iter;
		/* used to temp store the 3x3 matrices used to calculate the minors */
		float temp[9];

		/* find minor positions */
		int j;
		for ( int i = 0; i < 16; i++ )
		{
			row  = i / 4;
			col  = i - (row * 4);
			/* iter is used to fill minor[] */
			iter = 0;

			for ( j = 0; j < 16; j++ )
			{
				/* if j is not on row or col */
				if ( j / 4 != row && j - ((j / 4) * 4 ) != col )
				{
					temp[iter] = data[j];
					iter++;
				}
			}

			/* calculate minor */
			minorContainer[i] = ( temp[0] * ((temp[4] * temp[8]) - (temp[5] * temp[7])) )
								- ( temp[1] * ((temp[3] * temp[8]) - (temp[5] * temp[6])) )
								+ ( temp[2] * ((temp[3] * temp[7]) - (temp[4] * temp[6])) );
		}
	}
}
